package ballFinder.environment;

import lejos.nxt.Motor;

public class MotorControl {

	// Motor.A links
	// Motor.B rechts
	
	public enum TurnDirection {Left,Right};
		
	private final int speed = 400;
	private final int defaultAcceleration = 1000;
	private final int MAX_Acceleration = 6000;
	public final int rotateNinetyDegreeAngle = 390;
	
	public enum Action {Drive,TurnLeft,TurnRight};
	private Action lastAction;

	private void resetToDefault() {
		Motor.A.setSpeed(speed);
		Motor.B.setSpeed(speed);
		Motor.A.setAcceleration(defaultAcceleration);
		Motor.B.setAcceleration(defaultAcceleration);
	}

	public int getNinetyDegreeAngle() {
		return rotateNinetyDegreeAngle;
	}
	
	public MotorControl(World world) {
		Motor.A.setAcceleration(defaultAcceleration);
		Motor.B.setAcceleration(defaultAcceleration);
		lastAction=Action.Drive;
	}
	
	public MotorControl() {
		Motor.A.setAcceleration(defaultAcceleration);
		Motor.B.setAcceleration(defaultAcceleration);
		lastAction=Action.Drive;
	}

	public void move() {
		lastAction=Action.Drive;
		resetToDefault();
		
		Motor.A.setSpeed(speed);
		Motor.B.setSpeed(speed);
		
		Motor.A.forward();
		Motor.B.forward();
	}

	public void move(int degrees) {
		lastAction=Action.Drive;
		resetToDefault();
		
		Motor.A.rotate(degrees, true);
		Motor.B.rotate(degrees, true);
		waitForMotorStop();
	}
	
	public void stop() {
		resetToDefault();
		Motor.A.setSpeed(0);
		Motor.B.setSpeed(0);
		waitForMotorStop();
	}
	
	public void waitForMotorStop() {
		while(Motor.A.isMoving() || Motor.B.isMoving()){
			try{Thread.sleep(10);}catch(Exception e){}
		}
	}
	
	public void stop_now() {
		Motor.A.setAcceleration(MAX_Acceleration);
		Motor.B.setAcceleration(MAX_Acceleration);
		Motor.A.stop();
		Motor.B.stop();
		Motor.A.setAcceleration(defaultAcceleration);
		Motor.B.setAcceleration(defaultAcceleration);
	}

	public void turnNinetyDegree(TurnDirection direction) {
		stop();
		Motor.A.resetTachoCount();
		Motor.B.resetTachoCount();
		switch(direction){
			case Left:
				lastAction=Action.TurnLeft;
				Motor.B.setSpeed(speed);
				Motor.B.rotate(rotateNinetyDegreeAngle,true);
				break;
			case Right:
				lastAction=Action.TurnRight;
				Motor.A.setSpeed(speed);
				Motor.A.rotate(rotateNinetyDegreeAngle,true);
				break;
			default:
				break;
		}
		waitForMotorStop();
	}
	
	public void turn(TurnDirection direction,int angle) {
		stop();
		Motor.A.resetTachoCount();
		Motor.B.resetTachoCount();
		switch(direction){
			case Left:
				lastAction=Action.TurnLeft;
				Motor.A.setSpeed(0);
				Motor.B.setSpeed(speed);
				Motor.B.rotate(angle,true);
				break;
			case Right:
				lastAction=Action.TurnRight;
				Motor.B.setSpeed(0);
				Motor.A.setSpeed(speed);
				Motor.A.rotate(angle,true);
				break;
			default:
				break;
		}
		waitForMotorStop();
	}
	
	public  void turnForever(TurnDirection direction) {
		int speed_tmp=speed/15;
		
		Motor.A.setSpeed(speed_tmp);
		Motor.B.setSpeed(speed_tmp);
		Motor.A.setAcceleration(defaultAcceleration/2);
		Motor.B.setAcceleration(defaultAcceleration/2);

		Motor.A.resetTachoCount();
		Motor.B.resetTachoCount();
		switch(direction){
			case Left:
				lastAction=Action.TurnLeft;
				Motor.A.setSpeed(0);
				Motor.B.setSpeed(speed);
				Motor.B.forward();
				break;
			case Right:
				lastAction=Action.TurnRight;
				Motor.B.setSpeed(0);
				Motor.A.setSpeed(speed);
				Motor.A.forward();
				break;
			default:
				break;
		}
	}

	public void back() {
		back(1);
	}
	public void back(int durationInSeconds) {
		lastAction=Action.Drive;
		
		Motor.B.setSpeed(speed/2);
		Motor.A.setSpeed(speed/2);
		Motor.A.backward();
		Motor.B.backward();
		
		try {Thread.sleep(durationInSeconds*1000);} catch (Exception e) {}
		
		stop();
	}

	public Action getLastAction() {
		return lastAction;
	}
}
